
#include <std.h>
#include <stdio.h>
#include <string.h>
#include <log.h>
#include <tsk.h>
#include <ecm.h>
#include <gio.h>
#include "ti/pspiom/i2c/I2c.h"

#include "GPS.h"
#include "common.h"
#include "stdio.h"
#include "math.h"
#include "common_i2c.h"
#include "navconbioscfg.h"
#include "err.h"


int GPS_i2cRead(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG, Uint8 *buf, size_t len){

	int error = 0;
	//request to read register @ address *addr
	error = i2cWrite(outhd,GPS_ADDR,( unsigned char *)&REG,1,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}
	//perform read, write to user-defined buffer
    error = i2cRead(inhd,GPS_ADDR,( unsigned char *)buf,len,I2c_DEFAULT_READ);
	if(error!=IOM_COMPLETED){
		return error;
	}
	return error;
}

int GPS_i2cWrite(GIO_Handle outhd, Uint8 * txbuf, size_t msglen){
	return i2cWrite(outhd,GPS_ADDR,( unsigned char *)txbuf,msglen,I2c_DEFAULT_WRITE);
}

int GPS_process_data(NAVSOL *p, Uint8 *data){

	if(p==NULL || data==NULL){
		return -1;
	}
	data[9] = 0;
	p->iTOW = (Uint32)(data[9] << 24)|(data[8] << 16)|(data[7] << 8)|data[6];
	p->fTOW = (Int32)(data[13] << 24)|(data[12] << 16)|(data[11] << 8)|data[10];
	p->WEEK = (Int16)(data[15] << 8)|data[14];
	p->TimeLable = (float)((*p).iTOW * 1000) + (float)((*p).fTOW) / 1000;
	p->gpsFix = data[16];
	p->flags = data[17];
	p->ecefX = (Int32)(data[21] << 24)|(data[20] << 16)|(data[19] << 8)|data[18];
	p->ecefY = (Int32)(data[25] << 24)|(data[24] << 16)|(data[23] << 8)|data[22];
	p->ecefZ = (Int32)(data[29] << 24)|(data[28] << 16)|(data[27] << 8)|data[26];
	p->pAcc = (Uint32)(data[33] << 24)|(data[32] << 16)|(data[31] << 8)|data[30];

	p->ecefVX = (Int32)(data[37] << 24)|(data[36] << 16)|(data[35] << 8)|data[34];
	p->ecefVY = (Int32)(data[41] << 24)|(data[40] << 16)|(data[39] << 8)|data[38];
	p->ecefVZ = (Int32)(data[45] << 24)|(data[44] << 16)|(data[43] << 8)|data[42];
	p->sAcc =  (Uint32)(data[49] << 24)|(data[48] << 16)|(data[47] << 8)|data[46];

	p->pDOP = (Uint16)((data[51] << 8)|data[50]);
	p->numSV = data[53];
	p->Checksum = (Uint16)(data[59] << 8)|data[58];


	return 0;
}

int GPS_process_rawdata(RXMRAW_GEN *raw_gen,RXMRAW_SAT *raw_sat[], Uint8 *data){

	int i = 0;
	int j = 0;
	Uint32 tp = 0;
	if(raw_gen==NULL || data==NULL){
		return -1;
	}
	raw_gen->iTOW = (Uint32)(data[9] << 24)|(data[8] << 16)|(data[7] << 8)|data[6];
	raw_gen->WEEK = (Int16)(data[11] << 8)|data[10];
	raw_gen->numSV = data[12];
	for(i=0;i<raw_gen->numSV;i++)
	{
		//raw_sat[i]->cpMes = (double)(data[21+24*i] << 56)|(data[20+24*i] << 48)|(data[19+24*i] << 40)|(data[18+24*i] << 32)|(data[17+24*i] << 24)|(data[16+24*i] << 16)|(data[15+24*i] << 8)|data[14+24*i];
		memcpy(&raw_sat[i]->cpMes,&data[14],8);
		//raw_sat[i]->prMes = (double)(data[29+24*i] << 56)|(data[28+24*i] << 48)|(data[27+24*i] << 40)|(data[26+24*i] << 32)|(data[25+24*i] << 24)|(data[24+24*i] << 16)|(data[23+24*i] << 8)|data[22+24*i];
		memcpy(&raw_sat[i]->prMes,&data[22],8);
		//raw_sat[i]->doMes = (float)(data[33+24*i] << 24)|(data[32+24*i] << 16)|(data[31+24*i] << 8)|data[30+24*i];
		//tp = (Uint32)(data[33+24*i] << 24)|(data[32+24*i] << 16)|(data[31+24*i] << 8)|data[30+24*i];
		memcpy(&raw_sat[i]->doMes,&data[30],4);
		raw_sat[i]->svID = data[34+24*i];
		raw_sat[i]->mesQI = (Int8)data[35+24*i];
		raw_sat[i]->cno = (Int8)data[36+24*i];
		raw_sat[i]->lli = data[37+24*i];
	}

	return 0;
}

